#include "StdAfx.h"

/*==============================================================*/
/*			RADA.CPP				*/
/*	(c) V.B.Titov		 		28.04.94	*/
/*==============================================================*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "rada.h"
//#include <boost/scoped_array.hpp>

//extern FILE *test;

Everhardt::Everhardt(int nv, int nclass, int nor, int ni, int ll, double xl)
{
	double HH[48] = {
		0.212340538239152E+00, 		0.590533135559265E+00, 		0.911412040487296E+00,
		0.0985350857988264E+00,		0.3045357266463639E+00,		0.5620251897526139E+00,
		0.8019865821263918E+00,		0.9601901429485313E+00,		0.0562625605369221E+00,
		0.1802406917368924E+00,		0.3526247171131696E+00,		0.5471536263305554E+00,
		0.7342101772154105E+00,		0.8853209468390957E+00,		0.9775206135612875E+00,
		0.0362578128832095E+00,		0.1180789787899987E+00,		0.2371769848149604E+00,
		0.3818827653047059E+00,		0.5380295989189891E+00,		0.6903324200723622E+00,
		0.8238833438370047E+00,		0.9256126102908040E+00,		0.9855875903511235E+00,
		0.0252736203975203E+00,		0.0830416134474051E+00,		0.1691751003771814E+00,
		0.2777967151090320E+00,		0.4015027202328608E+00,		0.5318623869104160E+00,
		0.6599918420853348E+00,		0.7771593929561621E+00,		0.8753807748555569E+00,
		0.9479645488728194E+00,		0.9899817195383196E+00,		0.0186103650109879E+00,
		0.0614755408992690E+00,		0.1263051786933106E+00,		0.2098429717265625E+00,
		0.3078989982803983E+00,		0.4155560359786595E+00,		0.5274156139958823E+00,
		0.6378686027177611E+00,		0.7413764592942375E+00,		0.8327489886084423E+00,
		0.9074047753009974E+00,		0.9616018612603216E+00,		0.9926353489739107E+00
	};
	int mc[12] =	 {
		1,13,24,34,43,51,58,64,69,73,76,78
	};
	int nw[14]  =   {
		0,0,1,3,6,10,15,21,28,36,45,55,66,78
	};

	int NXI[78] =	 {
		2,		3,		4,		5,		6,		7,		8,		9,		10,		11,
		12,		13,		3,		6,		10,		15,		21,		28,		36,		45,
		55,		66,		78,		4,		10,		20,		35,		56,		84,		120,
		165,	220,	286,	5,		15,		35,		70,		126,	210,	330,
		495,	715,	6,		21,		56,		126,	252,	462,	792,	1287,
		7,		28,		84,		210,	462,	924,	1716,   8,		36,		120,
		330,	792,	1716,   9,		45,		165,	495,	1287,	10,		55,
		220,	715,	11,		66,		286,	12,		78,		13
	};
	int kd2, la, lc;

	int j, l, k, m, n;
	int jd, jdm, lb, ld, le;

	NV = nv; 
	NCLASS = nclass; 
	NOR = nor; 
	LL = ll; 
	XL = xl; 
	NI = ni;

	for(j=0; j<14; j++)
	{
		NW[j] = nw[j];
		H[j] = 0.0;
	}
	for(j=0; j<13; j++) 
		U[j] = W[j] = 0.0;

	for(j=0; j<12; j++) 
		MC[j] = mc[j];

	for(j=0; j<78; j++) 
		XI[j] = C[j] = D[j] = R[j] = 0.0;

	F1 = new double [43*NV];

	for(j = 0; j < 43 * NV; ++j) 
		F1[j] = 0.0;

	FJ = F1 + NV; 
	Y  = FJ + NV; 
	Z  = Y  + NV;
	B  = Z  + NV;
	BE = B  + 13*NV;
	BT = BE + 13*NV;

	KD  = (NOR - 3)/2;
	kd2 = KD / 2;
	KE  = KD + 1;
	KF  = KD + 2;
	PW  = 1.0/( (double)(KD + 3));
	NCL = NPQ = NES = 0;
	if (NCLASS == 1) 
		NCL = 1;
	if (NCLASS < 2)  
		NPQ = 1;

	if (NV == 1) 
		SR = 1.2; 
	else 
		SR = 1.5;

	if (LL < 0)	 
		NES = 1;

	N_CLASS = abs(NCLASS);

	for(n = 1, la = kd2 * kd2 - 1; n < KF; ++n, ++la)
	{
		H[n]	= HH[la];
		W[n-1]	= 1.0/((double) ((n+1)*(1 + (n+1)*(N_CLASS-1))));
		U[n-1]	= n+2;
	}
	W1 = 1.0/((double) N_CLASS);

	for(j = 0; j < KD; j++)
	{
		m = MC[j] - 1;
		for(l = j+1; l < KE; l++, m++)
			XI[m] = W[j] * NXI[m] / W[l];
	}

	C[0] = -H[1] * W[0];
	D[0] =  H[1] / W[1];
	R[0] =  1.0 /(H[2] - H[1]);

	for(k=2, la=0, lc=0; k < KE; k++)
	{
		lb = la; la = lc + 1; lc = NW[k+1] - 1;
		jd = lc - la - 1;
		C[la] = -H[k] * C[lb];
		C[lc] = (C[la-1] / W[jd] - H[k]) * W[jd+1];
		D[la] = H[1] * D[lb] * W[k-1] / W[k];
		D[lc] = (D[la-1] * W[k-1] + H[k]) / W[k];
		R[la] = 1.0 / (H[k+1] - H[1]);
		R[lc] = 1.0 / (H[k+1] - H[k]);

		if (k != 2)
		{
			for(l = 3; l <= k; l++)
			{
				ld = la + l - 2;
				le = lb + l - 3;
				jdm= ld - la - 1;
				C[ld] = (W[jdm+1] * C[le] / W[jdm]) - (H[k] * C[le+1]);
				D[ld] = (H[l-1] * D[le+1] + D[le]) * W[k-1] / W[k];
				R[ld] = 1.0 / (H[k+1] - H[l-1]);
			}
		}
	}
}

Everhardt::~Everhardt(void)
{
	delete [] F1;
}

void Everhardt::rada27(double *X, double *V, double TI, double TF, IForceInterface *pForceInterface)
{
	if(pForceInterface == NULL)
	{
		ASSERT(0);
		throw CCustomException(_T("empty force interface"));
	}

	int k, l;

	NSF = NPER = 0;
	TDIF = TF - TI;
	DIR = TDIF / fabs(TDIF);
	SS = pow(10.0, -LL);
	NL = NI + 30;

	/* Set a reasonable estimate to T based on experience */

	if (NES)
	{
		XL = TP = fabs(XL) * DIR;
	}
	else
		TP = (((double) NOR) /11.0) * pow(0.5, 0.4 * (double) LL) * DIR/2.0;

	if (TP / TDIF > 0.5) 
		TP = 0.5 * TDIF;

	double * v_ptr = V;
	double * bt_g_ptr = BT;
	double * b_g_ptr = B;
	double * be_g_ptr = BE;
	for(k=0; k<NV; k++, v_ptr++, bt_g_ptr++, b_g_ptr++, be_g_ptr++)
	{
		if (NCL) 
			*v_ptr = 0.0;
		double * bt_ptr = bt_g_ptr;
		double * b_ptr = b_g_ptr;
		double * be_ptr = be_g_ptr;
		for(l=0; l<KE; l++, b_ptr += NV, bt_ptr += NV, be_ptr += NV)
		{
			*b_ptr = *bt_ptr = 0.0;
		}
	}

	NF = 0;
	NCOUNT = 0;
	do
	{
		NS = 0;
		TM = TI;
		SM = 10000.0;
		force(X, V, TM, F1, pForceInterface); 
		NF++;
	} while(this->rada_maker(X, V, TF, pForceInterface));
}

int Everhardt::rada_maker(double *X, double *V, double TF, IForceInterface *pForceInterface)
{
	int j, k, l, m, n;
	double *be_i_ptr;
	double *b_i_ptr;
	double *bt_g_ptr, *bt_ptr;
	double *u_ptr, *r_ptr, *c_ptr;
	double s, q, res, hsum, temp, val, bdouble;
	double t, tval, t2, eps = 1.0e-10;

	int j2, la, jdm;

	while(1)
	{
		/* Loop to find the Beta-values from B-values */

		double * be_g_ptr = BE + NV * (KE-1);
		double * b_g_ptr = B + NV * (KE-1);
		for (k = 0;	k < NV; k++, be_g_ptr++, b_g_ptr++)
		{
			*be_g_ptr = (*b_g_ptr) / W[KE-1];
			double * be_ptr = BE + k;
			double * b_ptr = B + k; 
			double * w_ptr = W;
			for (j=0; j < KD; j++, be_ptr += NV, b_ptr += NV, w_ptr++)
			{
				*be_ptr = *b_ptr / (*w_ptr);
				for (l=j+1, b_i_ptr=b_ptr+NV; l<KE; l++, b_i_ptr+=NV)
				{
					n = NW[l] + j;
					*be_ptr += (*b_i_ptr) * D[n];
				}
			}
		}

		t = TP; 
		tval = fabs(t); 
		t2 = pow(t, N_CLASS);
		/* Iteration loop with NL=NI passes */

		for (m = 0; m < NL; m++)
		{
			for (j = 1, j2 = 1; j < KF; j++)
			{
				la = NW[j-1] - 1;
				jdm = j - 1;
				s = H[j];
				q = pow(s, N_CLASS - 1);
				if (NPQ)
				{
					b_g_ptr = B + (KE - 1) * NV;
					for (k=0; k<NV; k++, b_g_ptr++)
					{
						res = *b_g_ptr;
						double * b_ptr = b_g_ptr - NV;
						for (l=0; l < KD; l++, b_ptr -= NV)
						{
							res = *b_ptr + s * res;
						}
						Y[k] = q * (t * V[k] + (t2 * s * (F1[k] * W1 + s * res))) + X[k];
					}
				}
				else
				{
					b_g_ptr = B + (KE-1) * NV;
					for (k=0; k < NV; k++, b_g_ptr++)
					{
						res = *b_g_ptr;
						temp = res * U[KE-1]; 
						double * b_ptr = b_g_ptr - NV;
						u_ptr = U + KE - 2;
						for (l=0; l<KD; l++, b_ptr -= NV, u_ptr --)
						{
							res = (*b_ptr) + (s * res);
							temp = (*b_ptr) * (*u_ptr) + (s * temp);
						}
						Y[k] = q * ((t * V[k]) + (t2 * s * ((F1[k] * W1) + (s * res)))) + X[k];
						Z[k] = s * t * (s * temp + F1[k]) + V[k];
					}
				}
				force(Y, Z, TM + s * t, FJ, pForceInterface); 
				NF++;
				if (j2)
				{
					j2 = 0;
					double * be_ptr = BE; 
					double * b_ptr = B;
					for (k=0; k < NV; k++, be_ptr++, b_ptr++)
					{
						temp = *be_ptr;
						res = (FJ[k] - F1[k])/s;
						*be_ptr = res;
						*b_ptr += (res - temp) * W[0];
					}
				}
				else
				{
					double * be_ptr = BE + (j-1) * NV;
					double * b_ptr = B + (j-1) * NV;
					for (k = 0; k < NV;	k++, be_ptr++, b_ptr++)
					{
						temp = *be_ptr;
						res = (FJ[k] - F1[k])/s;
						r_ptr=R + la + 1;
						be_i_ptr = BE + k;
						for (l=0; l < jdm; l++, r_ptr++, be_i_ptr += NV)
						{
							res = (res - *be_i_ptr) * (*r_ptr);
						}

						*be_ptr = res;
						temp = res - temp;
						*b_ptr += temp * W[j-1];
						c_ptr = C + la + 1;
						b_i_ptr = B+k;
						for (l=0; l<jdm; l++, c_ptr++, b_i_ptr+=NV)
						{
							*b_i_ptr += *c_ptr * temp;
						}
					}
				}
			}
			if (m < NI-1) 
				continue;

			hsum = 0.0;
			val = pow(tval, -KE);
			double * b_ptr = B + (KE - 1) * NV;
			for (k=0; k<NV; k++, b_ptr++)
			{
				bdouble = *b_ptr;
				hsum += bdouble*bdouble;
			}
			hsum = val * sqrt(hsum);
			if (NSF) 
				continue;
			if (fabs(hsum-SM) <= (0.01 * hsum)) 
				break;

			SM = hsum;
		}
		if (NSF == 0)/* The proper starting value for t */
		{
			if (hsum != 0.0) 
				TP = pow(SS / hsum, PW) * DIR;
			if (NES)
			{
				TP = XL;
			}
			else
			{
				if (TP / t <= 1.0)
				{
					TP *= 0.8;
					NCOUNT ++;
					return (NCOUNT <= 10);
				}
			}
			NSF = 1;
		}
		/*  Find position and velocity for classes 2 and 2s */
		b_g_ptr = B + (KE-1) * NV;
		for (k = 0; k < NV; k++, b_g_ptr++)
		{
			res = *b_g_ptr;
			double * b_ptr = B + k;
			for (l=0; l < KD; l++, b_ptr += NV)
			{
				res += *b_ptr;
			}
			X[k] += (V[k] * t) + t2*(F1[k] * W1 + res);
			if (NCL) 
				continue;
			res = *b_g_ptr * U[KE-1];
			b_ptr = B + k;
			for (l=0; l<KD; l++, b_ptr+=NV)
			{
				res += *b_ptr * U[l];
			}
			V[k] += t * (F1[k] + res);
		}

		TM += t;
		NS ++;
		if (NPER) 
			return 0;

		/* Control on size of next sequence; return when TF reached */
		force( X, V, TM, F1, pForceInterface); 
		NF++;

		if (NES)
			TP = XL;
		else
		{
			if (hsum != 0.0) 
				TP = pow(SS/hsum, PW) * DIR;

			if (TP/t > SR) 
				TP = SR*t;
		}
		if (DIR*(TM+TP) >= DIR*TF - eps)
		{
			TP = TF - TM;
			NPER = 1;
		}
		q = TP/t;
		b_g_ptr = B;
		bt_g_ptr = BT;
		for (k=0; k<NV; k++, b_g_ptr++, bt_g_ptr++)
		{
			res = 1.0;
			double * b_ptr = b_g_ptr;
			bt_ptr = bt_g_ptr;
			for (j=0; j<KE;	j++, b_ptr += NV, bt_ptr += NV)
			{
				if (NS > 1) 
					*bt_ptr = *b_ptr - *bt_ptr;
				if (j < KE-1)
				{
					m = MC[j] - 1;
					b_i_ptr = b_ptr + NV;
					for (l=j+1; l<KE; l++, b_i_ptr += NV, m++)
					{
						*b_ptr += XI[m] * (*b_i_ptr);
					}
				}
				res *= q;
				temp = res * (*b_ptr);
				*b_ptr = temp + (*bt_ptr);
				*bt_ptr = temp;
			}
		}
		NL = NI;
	}
}

void Everhardt::force(double X[], double V[], double TS, double F[], IForceInterface *pForceInterface)
{
	ASSERT(pForceInterface);
	pForceInterface->Force(NV, TS, X, F);
}


/*==============================================================*/
/*							 EOF 								*/
/*==============================================================*/
